#include "SWD.h"

//0
#define R_Idcode 0xA5
#define W_Abort 0x81

//1
#define R_STAT 0x8D
#define W_CTRL 0xA9
//3
#define R_RDBUFF 0xBD
//2
#define W_Select 0xB1

#define R_CSW 0x87
#define W_CSW 0xA3

#define W_TAR 0x8B

#define R_DRW 0x9F
#define W_DRW 0xBB

#define BUS_SWD_ACK			1
#define BUS_SWD_WAIT		2
#define BUS_SWD_FAULT		4

#define AP_CSW         0x00        // Control and Status Word
#define AP_TAR         0x04        // Transfer Address
#define AP_DRW         0x0C        // Data Read/Write
#define AP_BD0         0x10        // Banked Data 0
#define AP_BD1         0x14        // Banked Data 1
#define AP_BD2         0x18        // Banked Data 2
#define AP_BD3         0x1C        // Banked Data 3
#define AP_ROM         0xF8        // Debug ROM Address
#define AP_IDR         0xFC        // Identification Register


#define ADDR_CPUID	0xE000ED00UL
#define ADDR_AIRCR	0xE000ED0CUL
#define ADDR_DFSR	0xE000ED30UL
#define ADDR_CPACR	0xE000ED88UL
#define ADDR_DHCSR	0xE000EDF0UL
#define ADDR_DCRSR	0xE000EDF4UL
#define ADDR_DCRDR	0xE000EDF8UL
#define ADDR_DEMCR	0xE000EDFCUL

#define DHCSR_KEY_MASK	0xFFFF0000UL
#define DHCSR_KEY_VAL	0xA05F0000UL
#define DHCSR_BIT_STEP	0x00000004UL
#define DHCSR_BIT_HALT	0x00000002UL
#define DHCSR_BIT_DBGEN	0x00000001UL


#define maxtry 16

u32 vid;
u32 cpuid;
u32 DP_SELECT;
u32 CSW_value;


const u8 writeAP[]={0xA3,0x8b,0x93,0xbb};
const u8 readAP[]= {0x87,0xaf,0xb7,0x9f};

//#define SWD_CLKDIO_CLR() GPIOD->BSRR=((BIT2|BIT1)<<16)
//#define SWD_CLKDIO_SET() GPIOD->BSRR=((BIT1<<16)|BIT2)

//#define asmtx

#ifdef asmtx
__asm void w8(u8 temp){
//	u8 i;
//  for(i=0;i<8;i++)
//  {
//		//nop();
//	  if (temp&1)
//		  SWD_CLKDIO_SET();//SWD_DIO_SET();
//	  else
//		  SWD_CLKDIO_CLR();//SWD_DIO_CLR();
//		//SWD_CLK_CLR();
//	  //nop();
//		temp >>= 1;
//	  SWD_CLK_SET();
//	  
//  }
	push {r1-r2,r6}
	movs r1,#0x20
	
	ldr r2,=0x48000C18
	ldr r6,=0x48000418
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n0
	strh r1,[r2,#0];
n0
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n1
	strh r1,[r2,#0];
n1
	strh r1,[r6,#2];
	strh r1,[r6,#0];

	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n2
	strh r1,[r2,#0];
n2
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n3
	strh r1,[r2,#0];
n3
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n4
	strh r1,[r2,#0];
n4
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n5
	strh r1,[r2,#0];
n5
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n6
	strh r1,[r2,#0];
n6
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc n7
	strh r1,[r2,#0];
n7
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	pop {r1-r2,r6}
	bx lr;
}

__asm u32 w32(u32 temp){
	push {r1-r4,r6}
	
	movs r1,#0x20;//bit5
	movs r3,#4;//count
	movs r4,#0;//odd
	
	
	ldr r2,=0x48000C18
	ldr r6,=0x48000418
	
n
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn0
	strh r1,[r2,#0];
	adds r4,r4,#1
xn0
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn1
	strh r1,[r2,#0];
	adds r4,r4,#1
xn1
	strh r1,[r6,#2];
	strh r1,[r6,#0];

	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn2
	strh r1,[r2,#0];
	adds r4,r4,#1
xn2
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn3
	strh r1,[r2,#0];
	adds r4,r4,#1
xn3
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn4
	strh r1,[r2,#0];
	adds r4,r4,#1
xn4
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn5
	strh r1,[r2,#0];
	adds r4,r4,#1
xn5
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn6
	strh r1,[r2,#0];
	adds r4,r4,#1
xn6
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	strh r1,[r2,#2];
	lsrs r0,r0,#1
	bcc xn7
	strh r1,[r2,#0];
	adds r4,r4,#1
xn7
	strh r1,[r6,#2];
	strh r1,[r6,#0];
	
	subs r3,r3,#1
	bne n
	mov r0,r4
	pop {r1-r4,r6}
	bx lr;
	nop
}


#else
void w8(u8 temp){
	u8 i;
  for(i=0;i<8;i++)
  {
		//nop();
	  if (temp&1)
		  SWD_DIO_SET();
	  else
		  SWD_DIO_CLR();
		SWD_CLK_CLR();
	  //nop();
	  SWD_CLK_SET();
	  temp >>= 1;
  }
}

u8 w32(u32 value){
	u8 i,addr=0;
	for(i=0;i<32;i++){
		if(value&1)
			SWD_DIO_SET();
		else
			SWD_DIO_CLR();
		SWD_CLK_CLR();
		addr+=value;
		value>>=1;
		SWD_CLK_SET();
	}
	return addr;
}

#endif

u8 wInt(u8 addr,u32 value){
	u8 i;
	w8(addr);
	//SWD_DIO_SET();
	addr=0;
	
	SWD_CLK_CLR();
	SWD_CLK_SET();
	
	SWD_IN();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT0;
	SWD_CLK_SET();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT1;
	SWD_CLK_SET();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT2;
	SWD_CLK_SET();
	
	SWD_OUT();
	if(addr!=1){
		SWD_CLK_CLR();
		SWD_CLK_SET();
		return addr?addr:1;
	}
	
	SWD_CLK_CLR();
	SWD_CLK_SET();


	if(w32(value)&1)
		SWD_DIO_SET();
	else
		SWD_DIO_CLR();

	SWD_CLK_CLR();
	SWD_CLK_SET();
		
	return 0;
}

u8 rInt(u8 addr,u32* value){
	u8 i;
	u32 val;
	w8(addr);
	//SWD_DIO_SET();
	addr=0;
	
	SWD_CLK_CLR();
	SWD_CLK_SET();
	
	SWD_IN();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT0;
	SWD_CLK_SET();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT1;
	SWD_CLK_SET();
	
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr|=BIT2;
	SWD_CLK_SET();
	
	if(addr!=1){
		SWD_CLK_CLR();
		SWD_CLK_SET();
		SWD_OUT();
		return addr?addr:1;
	}
	addr=0;
	val=0;
	for(i=0;i<32;i++){
		val>>=1;
		SWD_CLK_CLR();
		if(SWD_DIO())
		{
			val|=0x80000000;
			addr++;
		}
		SWD_CLK_SET();
	}
		
	SWD_CLK_CLR();
	if(SWD_DIO())
		addr++;
	SWD_CLK_SET();
	
	SWD_OUT();
	
	SWD_CLK_CLR();
	SWD_CLK_SET();
	
	
	
	if(addr&BIT0)return 0x80;
	*value=val;
	return 0;
}

void rst(){
	//w32(0xffffffff);
	w32(0xffffffff);
	w32(0xffffffff);
	w8(0x9e);
	w8(0xe7);
	w32(0xffffffff);
	w32(0x3fffffff);
	//w8(0);
	CSW_value=0;
	DP_SELECT=0;
}

u8 wBus(u8 cmd,u32 val){
	u8 i,ret;
	for(i=0;i<maxtry;i++)
	{
		ret=wInt(cmd,val);
		if(ret==BUS_SWD_WAIT)continue;
		else return ret;
	}
	return ret;
}

u8 rBus(u8 cmd,u32* val){
	u8 i,ret;
	for(i=0;i<maxtry;i++)
	{
		ret=rInt(cmd,val);
		if(ret==BUS_SWD_WAIT)continue;
		else return ret;
	}
	return ret;
}



u32 getIDCores(){
	return vid;
}


u8 WriteDPSelect(u32 value){
	value&=0xf0;
	if(DP_SELECT==value)return 0;
	DP_SELECT=value;
	return wBus(W_Select,DP_SELECT);
}

u8 WriteCSW(u32 value){
	u32 dummy;
	WriteDPSelect(0);
	if(value==CSW_value)return 1;
	CSW_value=value;
	if(wBus(W_CSW,value))return 0;
	return rBus(R_RDBUFF,&dummy)==0;
}
u8 ReadCSW(u32* value){
	WriteDPSelect(0);
	if(rBus(R_DRW,value))return 0;
	if(rBus(R_RDBUFF,value))return 0;
	return 1;
}

u8 WriteDAP(u8 addr,u32 value){
	u32 dummy;
	if(addr==0)CSW_value=value;
	WriteDPSelect(addr);
	if(wBus(writeAP[(addr&0xf)>>2],value))return 0;
	return rBus(R_RDBUFF,&dummy)==0;
}
u8 ReadDAP(u8 addr,u32* value){
	WriteDPSelect(addr);
	if(rBus(readAP[(addr&0xf)>>2],value))return 0;
	if(rBus(R_RDBUFF,value))return 0;
	return 1;
}

u8 ReadData(u32 addr,u32* value){
	if(wBus(W_TAR,addr))return 0;
	if(rBus(R_DRW,value))return 0;
	if(rBus(R_RDBUFF,value))return 0;
	return 1;
}

u8 WriteData(u32 addr,u32 value){
	u32 dummy;
	if(wBus(W_TAR,addr))return 0;
	if(wBus(W_DRW,value))return 0;
	if(rBus(R_RDBUFF,&dummy))return 0;
	return 1;
}

u8 SetDataLenght(u8 size){
	return WriteCSW((CSW_value&~0x7)|size);
}

u8 WriteMem(u32 addr,s16 len,u32* data){
#if 1
	u32 dummy;
	u8 count=1<<(CSW_value&3);
	u8 b4=(u8)addr;
	//len/=4;
	//addr&=0xfffffffc;
	if(wBus(W_TAR,addr))return 0;
	if(len){
		dummy=*data++;
		do{
			if(wBus(W_DRW,dummy))return 0;
			len-=count;
			if(!((b4+=count)&3))
				dummy=*data++;
		}while(len>0);
	}
//	while(len){
//		len-=count;
//		dummy=*data++;
//		for(i=0;i<count;i++)
//			if(wBus(W_DRW,dummy))return 0;
//	}
	if(rBus(R_RDBUFF,&dummy))return 0;
#else
	u32 dummy;
	len/=4;
	addr&=0xfffffffc;
	if(wBus(W_TAR,addr))return 0;
	while(len--)
		if(wBus(W_DRW,*data++))return 0;
	if(rBus(R_RDBUFF,&dummy))return 0;
#endif
	return 1;
}

u8 ReadMem(u32 addr,s16 len,u32* data){
#if 1
	u32 dummy;
	u32 temp=0;
	u8 b4=(u8)addr;
	u8 count=1<<(CSW_value&3);
	//len/=4;
	//addr&=0xfffffffc;
	if(wBus(W_TAR,addr))return 0;
	if(rBus(R_DRW,data))return 0;
	if(len){
		do{
			if(rBus(R_DRW,&dummy))return 0;
			temp|=dummy;
			len-=count;
			if(!((b4+=count)&3)){
				*data++=temp;
				temp=0;
			}
		}while(len>0);
		*data=temp;
	}
//	while(len--)
//		if(rBus(R_DRW,data++))return 0;
#else
	len/=4;
	addr&=0xfffffffc;
	if(wBus(W_TAR,addr))return 0;
	if(rBus(R_DRW,data))return 0;
	while(len--)
		if(rBus(R_DRW,data++))return 0;
#endif
	return 1;
}



u8 ReadReg(u8 reg,u32 *value){
	if(!WriteData(ADDR_DCRSR,reg))return 0;
	return ReadData(ADDR_DCRDR,value);
}

u8 WriteReg(u8 reg,u32 value){
	u32 tmp;
	if(!WriteData(ADDR_DCRDR,value))return 0;
	if(!WriteData(ADDR_DCRSR,reg|0x10000))return 0;
	if(!ReadReg(reg,&tmp))return 0;
	return value==tmp;
}

u8 Halt(){
	return WriteData(ADDR_DHCSR,DHCSR_KEY_VAL | DHCSR_BIT_HALT | DHCSR_BIT_DBGEN);
}
u8 isHalt(){
	u32 tmp;
	if(!ReadData(ADDR_DHCSR,&tmp))return 1;
	return (tmp&0x00020000)!=0;
}

u8 Go(){
	return WriteData(ADDR_DHCSR,DHCSR_KEY_VAL | DHCSR_BIT_DBGEN);
}

u8 Run(){
	return WriteData(ADDR_DHCSR,DHCSR_KEY_VAL);
}

u8 Reset(){
	return WriteData(ADDR_AIRCR,0x05FA0004);
}

void init(){
//	GPIO_EN(GPIOB);
//	GPIO_EN(GPIOD);
//	SETBIT(RCC->APB1ENR,RCC_APB1ENR_IOMUXEN);
//	
//	SETBIT(GPIOD->OSPEEDR,(3<<10));
//	SETBIT(GPIOB->OSPEEDR,(3<<10));
//	
//	SETBIT(GPIOD->PUPDR,(1<<10));
//	SETBIT(GPIOB->PUPDR,(1<<10));
//	
//	SETBIT(GPIOB->ODR,BIT5);
//	SETBIT(GPIOD->ODR,BIT5);
//	
//	GPIO_PP(GPIOB,0,00100000);
//	GPIO_PP(GPIOD,0,00100000);
	//CLRBIT(GPIOB->ODR,0xf0);
	
	
}

u8 preReset(){
	u32 val;
	if(!Halt())return 0;//
	rBus(R_STAT,&val);//
	if(!WriteData(ADDR_DEMCR, 0x00000001UL))return 0;//2???��??t
	rBus(R_STAT,&val);//
	if(!Reset())return 0;//?a��??��??
	return 1;
}

u8 endReset(){
	if(!SWD.SetReg(16,0x01000000))return 0;
	if(!SWD.WriteData(0xE000EDFCUL, 0x01000000UL))return 0;
	return 1;
}


u8 setup(){
	u8 i;
	u32 val;
	if(wBus(W_Abort,0x1E))return 0;
	if(wBus(W_Select,0))return 0;
	if(wBus(W_CTRL,0x50000000UL))return 0;
	for(i=0;i<maxtry;i++){
		if(rBus(R_STAT,&val))return 0;
		if ((uint8_t)(val >> 28) == 0xF)
			break;
	}
	if(i==maxtry)return 0;
	//if(wBus(W_CTRL,0xF0000F00UL))return 0;
	if(wBus(W_Select,0))return 0;
	if(!WriteCSW(0x23000012))return 0;
	if(!ReadData(0xE000ED00UL,&cpuid))return 0;
	rBus(R_STAT,&val);
	//if(!WriteData(0xe000edf0,DBGKEY | C_DEBUGEN));
	
	
	
	//if(!Go())return 0;
	//rBus(R_STAT,&val);
	//if(!ReadData(ADDR_DEMCR,&vid))return 0;
	
	//Go();

	return preReset();
}

u8 connect(){
	u8 i;
	u32 val;
	rst();
	if(rBus(R_Idcode,&vid))return 0;
	if(wBus(W_Abort,0x1E))return 0;
	if(wBus(W_Select,0))return 0;
	if(wBus(W_CTRL,0x50000000UL))return 0;
	for(i=0;i<maxtry;i++){
		if(rBus(R_STAT,&val))return 0;
		if ((uint8_t)(val >> 28) == 0xF)
			break;
	}
	if(i==maxtry)return 0;
	if(!WriteCSW(0x23000012))return 0;
	return Go();
}

const SWDBase SWD = {
	init,	
	connect,
	setup,
	Reset,
	Halt,
	isHalt,
	Go,
	Run,
	WriteReg,
	ReadReg,
	WriteData,
	ReadData,
	WriteMem,
	ReadMem,
	preReset,
	endReset,
	ReadDAP,
	WriteDAP,
	getIDCores,
	SetDataLenght,
};
